import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    port_arg = DeclareLaunchArgument(
        'port',
        default_value='/dev/ttyACM0',
        description='Port for the so101 follower arm.'
    )
    port_config = LaunchConfiguration('port')

    # 终端 A: 硬件接口
    hw_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('so101_hw_interface'),
                'launch',
                'so101_hw.launch.py'
            ])
        ),
        launch_arguments={
            'port': port_config
        }.items()
    )

    # 终端 B: MoveIt
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('lerobot_moveit'),
                'launch',
                'so101_moveit.launch.py'
            ])
        ),
        # 传递 display:=False 参数
        launch_arguments={'display': 'False'}.items()
    )

    # 终端 C: 手柄控制
    controller_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('joystick_alphadog_with_so101_servo'),
                'launch',
                'unified_control.launch.py'
            ])
        ),
        launch_arguments={'use_sim': 'False'}.items()
    )

    return LaunchDescription([
        port_arg,
        hw_launch,
        moveit_launch,
        controller_launch
    ])
